#include <iostream>
#include <string>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/common/transforms.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/print.h>
#include <boost/thread/thread.hpp>
#include <Eigen/Core>
#include <pcl/point_cloud.h>
#include <pcl/common/time.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/fpfh_omp.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/sample_consensus_prerejective.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <libgen.h>

typedef pcl::PointXYZ PointT;
typedef pcl::PointNormal PointNT;
typedef pcl::PointCloud<PointT> PointCloudT;
typedef pcl::PointCloud<PointNT> PointCloudNT;
typedef pcl::FPFHSignature33 FeatureT;
typedef pcl::FPFHEstimationOMP<PointT,PointNT,FeatureT> FeatureEstimationT;
typedef pcl::PointCloud<FeatureT> FeatureCloudT;

typedef pcl::visualization::PointCloudColorHandlerCustom<PointT> ColorHandlerT;
std::string plyPath;
float rotX_ang,rotY_ang,rotZ_ang;//rotation paras
float movX_pos,movY_pos,movZ_pos;//translation paras
PointCloudT::Ptr sourceCloud(new PointCloudT());
PointCloudT::Ptr dispCloud(new PointCloudT());
PointCloudT::Ptr transCloud(new PointCloudT());
PointCloudT::Ptr ransacAlignCloud (new PointCloudT());
PointCloudT::Ptr icpAlignCloud (new PointCloudT());
Eigen::Affine3f mouse_Transform(Eigen::Affine3f::Identity());

pcl::SampleConsensusPrerejective<PointT,PointT,FeatureT> align;
pcl::IterativeClosestPoint<PointT,PointT> icp;

int loadPCDFile(PointCloudT::Ptr sourceCloud)
{
  if(pcl::io::loadPLYFile(plyPath+"/ply/rabbit.ply",*sourceCloud)<0)
    {
      PCL_ERROR("Error loading ply file!\n");
      return -1;
    }

  //DownSampling
  pcl::VoxelGrid<PointT> grid;
  grid.setLeafSize(0.5,0.5,0.5);
  grid.setInputCloud(sourceCloud);
  grid.filter(*sourceCloud);

  return 1;
}

//generate 3 faces of a cube model
int generatePointCloud(int width,int height,PointCloudT::Ptr cloud)
{
  if(width<=0 || height<=0)
  {
    return -1;
  }
  cloud->width=width;
  cloud->height=1;
  cloud->points.resize(width*height);
  for(size_t i=0;i<cloud->points.size();++i)
  {
    if(i<cloud->points.size()/3)
    {
      cloud->points[i].x=0.4*rand()/(RAND_MAX+1.0f);
      cloud->points[i].y=0.2*rand()/(RAND_MAX+1.0f);
      cloud->points[i].z=0.2+0.01*rand()/(RAND_MAX+1.0f);
    }
    else if(i<cloud->points.size()*2/3)
    {
      cloud->points[i].x=0.4+0.01*rand()/(RAND_MAX+1.0f);
      cloud->points[i].y=0.2*rand()/(RAND_MAX+1.0f);
      cloud->points[i].z=0.2*rand()/(RAND_MAX+1.0f);
    }
    else
    {
      cloud->points[i].x=0.4*rand()/(RAND_MAX+1.0f);
      cloud->points[i].y=0.2+0.01*rand()/(RAND_MAX+1.0f);
      cloud->points[i].z=0.2*rand()/(RAND_MAX+1.0f);
    }
  }
  return 1;
}

//inputType=0,load from pcd file;
//inputType=1,generate new cloud;
void getSourcePointCloud(PointCloudT::Ptr sourceCloud,int inputType)
{
  if(inputType==0)
  {
    if(loadPCDFile(sourceCloud)<0)
    {
        generatePointCloud(100000,1,sourceCloud);
    }
  }
  else
  {
    generatePointCloud(100000,1,sourceCloud);
  }

  dispCloud->width=sourceCloud->width;
  dispCloud->height=sourceCloud->height;
  dispCloud->points.resize(sourceCloud->points.size());
  for(int i=0;i<dispCloud->points.size();++i)
  {
    dispCloud->points[i].x=sourceCloud->points[i].x;
    dispCloud->points[i].y=sourceCloud->points[i].y;
    dispCloud->points[i].z=sourceCloud->points[i].z;
  }

  transCloud->width=sourceCloud->width;
  transCloud->height=sourceCloud->height;
  transCloud->points.resize(sourceCloud->points.size());
  for(int i=0;i<transCloud->points.size();++i)
  {
    transCloud->points[i].x=sourceCloud->points[i].x;
    transCloud->points[i].y=sourceCloud->points[i].y;
    transCloud->points[i].z=sourceCloud->points[i].z;
  }
}

void translateCloud(PointCloudT::Ptr cloud,float posX,float posY,float posZ)
{
  
}

void rotateCloud(PointCloudT::Ptr cloud,float angle,int axis)
{
  if(cloud->points.size()==0)
    return;
  Eigen::Matrix4f transform=Eigen::Matrix4f::Identity();
  transform(0,0)=cos(angle);
  transform(0,1)=-sin(angle);
  transform(1,0)=sin(angle);
  transform(1,1)=cos(angle);
  pcl::transformPointCloud(*cloud,*dispCloud,transform);
}

bool register_ransac()
{
    pcl::ScopeTime t("RANSAC Alignment");
    std::cout<<std::endl<<"============================="<<std::endl;
    pcl::console::print_highlight ("Starting Ransac Alignment...\n");

    /*
    //DownSampling
    pcl::VoxelGrid<PointT> grid;
    grid.setLeafSize(0.01,0.01,0.01);
    grid.setInputCloud(sourceCloud);
    grid.filter(*sourceCloud);
    grid.setInputCloud(transCloud);
    grid.filter(*transCloud);
    */


   // Estimate normals
    pcl::NormalEstimationOMP<PointT,PointNT> nest;
    nest.setRadiusSearch (2);
    PointCloudNT::Ptr sourceNormals(new PointCloudNT);
    PointCloudNT::Ptr transNormals(new PointCloudNT);
    nest.setInputCloud (sourceCloud);
    nest.compute (*sourceNormals);
    nest.setInputCloud (transCloud);
    nest.compute (*transNormals);

    FeatureCloudT::Ptr sourceCloud_features(new FeatureCloudT);
    FeatureCloudT::Ptr transCloud_features(new FeatureCloudT);
    // Estimate features
    //pcl::console::print_highlight ("Estimating features...\n");
    FeatureEstimationT fest;
    fest.setRadiusSearch (2);
    fest.setInputCloud (sourceCloud);
    fest.setInputNormals (sourceNormals);
    fest.compute (*sourceCloud_features);
    fest.setInputCloud (transCloud);
    fest.setInputNormals (transNormals);
    fest.compute (*transCloud_features);

    //Perform alignment
    const float leaf = 0.2f;
    align.setInputSource(sourceCloud);
    align.setSourceFeatures(sourceCloud_features);
    align.setInputTarget(transCloud);
    align.setTargetFeatures(transCloud_features);
    align.setMaximumIterations(500);
    align.setNumberOfSamples(3);
    align.setCorrespondenceRandomness(5);
    align.setSimilarityThreshold(0.9f);
    align.setMaxCorrespondenceDistance(3.0f*leaf);
    align.setInlierFraction(0.25f);

    align.align(*ransacAlignCloud);
    if(align.hasConverged())
    {
        pcl::console::print_highlight ("Ransac Alignment has converged");
        std::cout<<std::endl<<"Ransac Fitness score: "<<align.getFitnessScore()<<std::endl;
        std::cout<<"ransac transform matrix:"<<std::endl<<align.getFinalTransformation()<<std::endl;
        return true;
    }
    else
    {
        pcl::console::print_error ("Ransac Alignment stopped without convergence!\n");
        return false;
    }
}

bool register_icp()
{
    pcl::ScopeTime t("ICP Alignment");
    std::cout<<std::endl<<"============================="<<std::endl;
    pcl::console::print_highlight ("Starting ICP Alignment...\n");

    icp.setMaximumIterations(5000);
    icp.setInputSource(ransacAlignCloud);
    icp.setInputTarget(transCloud);
    icp.align(*icpAlignCloud);

    if(icp.hasConverged())
    {
        pcl::console::print_highlight ("ICP Alignment has converged");
        std::cout<<std::endl<<"ICP Fitness score: "<<icp.getFitnessScore()<<std::endl;
        std::cout<<"ICP transform matrix:"<<std::endl<<icp.getFinalTransformation()*align.getFinalTransformation()<<std::endl;
        return true;
    }
    else
    {
        pcl::console::print_error ("ICP Alignment stopped without convergence!\n");
        return false;
    }
}

void MouseEventOccurred(const pcl::visualization::MouseEvent &event, void* viewer_void)
{

  pcl::visualization::PCLVisualizer *viewer=static_cast<pcl::visualization::PCLVisualizer *>(viewer_void);
  Eigen::Affine3f viewPos=viewer->getViewerPose(0);
  if(event.getButton()==pcl::visualization::MouseEvent::LeftButton &&
      event.getType()==pcl::visualization::MouseEvent::MouseButtonRelease)
  {
    mouse_Transform=viewPos.rotation();
    mouse_Transform=mouse_Transform*Eigen::AngleAxisf(M_PI,Eigen::Vector3f::UnitY());
    mouse_Transform=mouse_Transform.inverse();
    pcl::transformPointCloud(*sourceCloud,*transCloud,mouse_Transform);
    //Align
    {
        pcl::ScopeTime t("Total time");
        if(register_ransac())
        {
            if(register_icp())
            {
                std::cout<<std::endl<<"============================="<<std::endl;
                std::cout<<"Align Converged..."<<std::endl;
            }
        }
    }
    std::cout<<"Real transform matrix:"<<std::endl<<mouse_Transform.matrix()<<std::endl;
  }
}

void keyboardEventOccurred(const pcl::visualization::KeyboardEvent &event,void* viewer_void)
{
  if(event.getKeySym()=="w" && event.keyDown())//+x
  {
    rotX_ang+=0.01;
    if(rotX_ang>2*M_PI)
      rotX_ang=0;
    rotateCloud(sourceCloud,rotX_ang,0);
    std::cout<<"Rotate by +x, angle= "<<rotX_ang<<std::endl;
  }
  else if(event.getKeySym()=="s" && event.keyDown())//-x
  {
    rotX_ang-=0.01;
    if(rotX_ang<=0)
      rotX_ang=2*M_PI;
    rotateCloud(sourceCloud,rotX_ang,0);
    std::cout<<"Rotate by -x, angle= "<<rotX_ang<<std::endl;
  }
  else if(event.getKeySym()=="a" && event.keyDown())//+y
  {
  }
  else if(event.getKeySym()=="d" && event.keyDown())//-y
  {
  }
}

int main(int argc, char** argv)
{
  plyPath=dirname(argv[0]);

  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("Interactive Viewer"));
  viewer->registerKeyboardCallback(keyboardEventOccurred,(void*)viewer.get());
  viewer->registerMouseCallback(MouseEventOccurred,(void*)viewer.get()); 
  viewer->setBackgroundColor(0,0,0);
  viewer->addCoordinateSystem(0.1);

  int v1(0);
  int v2(1);
  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer2(new pcl::visualization::PCLVisualizer("Align Results Viewer"));
  viewer2->setBackgroundColor(0,0,0);
  viewer2->addCoordinateSystem(0.1);
  viewer2->createViewPort(0.0,0.0,0.5,1.0,v1);
  viewer2->createViewPort(0.5,0.0,1.0,1.0,v2);
  
  getSourcePointCloud(sourceCloud,0);
  ColorHandlerT colorHandler(dispCloud,200,200,200);
  ColorHandlerT colorHandler21(transCloud,100,100,200);
  ColorHandlerT colorHandler22(transCloud,200,100,100);
  ColorHandlerT colorHandler23(transCloud,100,200,100);
  viewer->addPointCloud<PointT>(dispCloud,colorHandler,"disp_Cloud");

  float bckgr_gray_level=0.0;
  float txt_gray_lvl=1.0-bckgr_gray_level;
  viewer2->addPointCloud<PointT>(transCloud,colorHandler21,"trans_Cloud",v1);
  viewer2->addPointCloud<PointT>(icpAlignCloud,colorHandler22,"icp_align_Cloud",v2);
  viewer2->addPointCloud<PointT>(ransacAlignCloud,colorHandler23,"ransac_align_Cloud",v2);
  viewer2->addPointCloud<PointT>(transCloud,colorHandler21,"real_Cloud",v2);
  viewer2->addText("Blue: real pointCloud\nGreen: ransac aligned pointCloud\nPink: icp aligned pointCloud",10,15,16,txt_gray_lvl,txt_gray_lvl,txt_gray_lvl,"info_1",v2);

  while(!viewer->wasStopped())
  {
    viewer->updatePointCloud<PointT>(dispCloud,colorHandler,"disp_Cloud");
    viewer2->updatePointCloud<PointT>(transCloud,colorHandler21,"trans_Cloud");
    viewer2->updatePointCloud<PointT>(icpAlignCloud,colorHandler22,"icp_align_Cloud");
    viewer2->updatePointCloud<PointT>(ransacAlignCloud,colorHandler23,"ransac_align_Cloud");
    viewer2->updatePointCloud<PointT>(transCloud,colorHandler21,"real_Cloud");
    viewer->spinOnce(100);
    viewer2->spinOnce(100);
    boost::this_thread::sleep(boost::posix_time::microseconds(10000));
  }
  
}
